#include "geometryutilities.h"
#include <qmath.h>
#include <QDebug>
#include "utilities.h"

GeometryUtilities::Point GeometryUtilities::LineLineIntersect
    (const GeometryUtilities::Line &l1,
     const GeometryUtilities::Line &l2,
     GeometryUtilities::IntersectionInfo &info)
{
  if (!l1.isVertical() && !l2.isVertical())
  {
    qreal x1 = 0, x2 = 1, x3 = 0, x4 = 1;
    qreal y1 = l1.slope()*x1+l1.yIntercept(),
          y2 = l1.slope()*x2+l1.yIntercept(),
          y3 = l2.slope()*x3+l2.yIntercept(),
          y4 = l2.slope()*x4+l2.yIntercept();

    qreal denom = (y4-y3)*(x2-x1)-(x4-x3)*(y2-y1),
          numa = (x4-x3)*(y1-y3)-(y4-y3)*(x1-x3),
          numb = (x2-x1)*(y1-y3)-(y2-y1)*(x1-x3);
    qreal ua = numa/denom;
    // ub could be used in calculating the point to return if instead
    // of x1, x2, y1, y2; x3, x4, y3, y4 where used.
    //qreal ub = numb/denom;

    GeometryUtilities::Point p(x1+ua*(x2-x1), y1+ua*(y2-y1));
    if (Utilities::floatEquality(denom,0) &&
        Utilities::floatEquality( numa,0) &&
        Utilities::floatEquality(numb,0)) // Coincident
    {
      info = COINCIDENT;
    }
    else if (Utilities::floatEquality(denom,0)) // Parallel
    {
      info = PARALLEL;
    }
    else
    {
      info = INTERSECTION;
    }
    return p;
  }
  else if (l1.isVertical() && !l2.isVertical())
  {
    GeometryUtilities::Point p(l1.xIntercept(), l2.slope()*l1.xIntercept()+l2.yIntercept());
    info = INTERSECTION;
    return p;
  }
  else if (!l1.isVertical() && l2.isVertical())
  {
    GeometryUtilities::Point p(l2.xIntercept(), l1.slope()*l2.xIntercept()+l1.yIntercept());
    info = INTERSECTION;
    return p;
  }
  else
  {
    if (Utilities::floatEquality(l1.xIntercept(), l2.xIntercept()))
    {
      info = COINCIDENT;
    }
    else
    {
      info = NO_INTERSECTION;
    }
    return GeometryUtilities::Point(0.0,0.0);
  }
}

GeometryUtilities::Point GeometryUtilities::LineRayIntersect
    (const GeometryUtilities::Line &l,
     const GeometryUtilities::Ray &r,
     GeometryUtilities::IntersectionInfo &info)
{
  Point p1 = r.start(),
        p2 = r.start() + (r.direction() * 10);

  Point lli = LineLineIntersect
      (l, Line(p1, p2), info);
  if (info == INTERSECTION && r.isPointOn(lli))
  {
    return lli;
  }
  else
  {
    info = NO_INTERSECTION;
    return Point();
  }
}

GeometryUtilities::Point GeometryUtilities::RayLineIntersect
    (const GeometryUtilities::Ray& r,
     const GeometryUtilities::Line& l,
     GeometryUtilities::IntersectionInfo& info)
{
  return LineRayIntersect(l, r, info);
}

GeometryUtilities::Point GeometryUtilities::LineLineSegmentIntersect
    (const GeometryUtilities::Line& l,
     const GeometryUtilities::LineSegment& ls,
     GeometryUtilities::IntersectionInfo& info)
{
  Line l2(ls.begin(), ls.end());
  Point p = LineLineIntersect(l, l2, info);
  if (info == INTERSECTION && ls.isPointOn(p))
  {
    return p;
  }
  else
  {
    info = NO_INTERSECTION;
    return Point(0,0);
  }
}

GeometryUtilities::Point GeometryUtilities::LineSegmentLineIntersect
    (const GeometryUtilities::LineSegment& ls,
     const GeometryUtilities::Line& l,
     GeometryUtilities::IntersectionInfo& info)
{
  return LineLineSegmentIntersect(l, ls, info);
}

GeometryUtilities::Point GeometryUtilities::RayRayIntersect
    (const GeometryUtilities::Ray &r1,
     const GeometryUtilities::Ray &r2,
     GeometryUtilities::IntersectionInfo &info)
{
  Point p1b = r1.start(),
        p1e = r1.start() + (r1.direction() * 10),
        p2b = r2.start(),
        p2e = r2.start() + (r2.direction() * 10);
  Line l1(p1b, p1e),
       l2(p2b, p2e);
  Point lli = LineLineIntersect(l1, l2, info);
  if (info == INTERSECTION && r1.isPointOn(lli) && r2.isPointOn(lli))
    return lli;
  else
  {
    info = NO_INTERSECTION;
    return Point();
  }
}

GeometryUtilities::Point GeometryUtilities::RayLineSegmentIntersect
    (const GeometryUtilities::Ray &r,
     const GeometryUtilities::LineSegment &ls,
     GeometryUtilities::IntersectionInfo &info)
{
  Point p1 = r.start(),
        p2 = r.start() + (r.direction() * 10);
  Line l1(p1, p2), l2(ls.begin(), ls.end());
  Point lli = LineLineIntersect
      (l1, l2, info);
  if (info == INTERSECTION && r.isPointOn(lli) && ls.isPointOn(lli))
  {
    return lli;
  }
  else
  {
    info = NO_INTERSECTION;
    return Point();
  }
}

GeometryUtilities::Point GeometryUtilities::LineSegmentRayIntersect
    (const GeometryUtilities::LineSegment &ls,
     const GeometryUtilities::Ray &r,
     GeometryUtilities::IntersectionInfo &info)
{
  return RayLineSegmentIntersect(r, ls, info);
}

GeometryUtilities::Point GeometryUtilities::LineSegmentLineSegmentIntersect
    (const GeometryUtilities::LineSegment &ls1,
     const GeometryUtilities::LineSegment &ls2,
     GeometryUtilities::IntersectionInfo &info)
{
  Line l1(ls1.begin(), ls1.end()),
       l2(ls2.begin(), ls2.end());
  Point p = LineLineIntersect(l1, l2, info);
  //p.setX(qRound(p.x()));
  //p.setY(qRound(p.y()));
  if (info == INTERSECTION && ls1.isPointOn(p) && ls2.isPointOn(p))
  {
    return p;
  }
  else if (info == INTERSECTION)
  {
    info = NO_INTERSECTION;
  }
  return Point();
}

void GeometryUtilities::CircleCircleIntersect(const Circle &c1, const Circle &c2,
                                              Point(&pts)[2],
                                              GeometryUtilities::IntersectionInfo& info)
{
  qreal d = (c1.center() - c2.center()).magnitude();
  if (d > c1.radius() + c2.radius()) // separate
  {
    info = NO_INTERSECTION;
  }
  else if (d < qAbs(c1.radius()-c2.radius())) // containment
  {
    info = CONTAINED;
  }
  else if (Utilities::floatEquality(d, 0) &&
           Utilities::floatEquality(c1.radius(), c2.radius())) // coincident
  {
    info = COINCIDENT;
  }
  else
  {
    qreal a = (c1.radius()*c1.radius()-c2.radius()*c2.radius()-d*d)/(2*d);
    qreal h = qSqrt(c1.radius()*c1.radius()-a*a);
    Point p = c1.center()+(c2.center()-c1.center())/d;
    pts[0].setX(p.x()+h*(c2.center().y()-c1.center().y())/d);
    pts[0].setY(p.y()+h*(c2.center().x()+c1.center().x())/d);
    pts[1].setX(p.x()-h*(c2.center().y()+c1.center().y())/d);
    pts[1].setY(p.y()-h*(c2.center().x()-c1.center().x())/d);
    info = INTERSECTION;
  }
}

// Axis-aligned Bounding Box (AABB)
// pts[0] is nearest to ray origin, pts[1] is farthest.
//(Yes, there are algorithms out there that will do this a helluva lot quicker.)
void GeometryUtilities::RayRectIntersect(const Ray &r, const QRect &rect, Point (&pts)[2], IntersectionInfo& info)
{
  bool foundFirst = false, foundSecond = false;
  Point p1, p2, p;
  LineSegment ls1( Point(rect.topLeft()), Point(rect.bottomLeft()) );
  IntersectionInfo inf;
  p = RayLineSegmentIntersect(r, ls1, inf);
  if (inf != NO_INTERSECTION)
  {
    foundFirst = true;
    info = INTERSECTION;
    p1 = p;
  }

  LineSegment ls2( Point(rect.bottomLeft()), Point(rect.bottomRight()) );
  p = RayLineSegmentIntersect(r, ls2, inf);
  if (inf != NO_INTERSECTION)
  {
    if (!foundFirst)
    {
      foundFirst = true;
      p1 = p;
      info = INTERSECTION;
    }
    else
    {
      foundSecond = true;
      p2 = p;
    }
  }

  // Already found 2 intersections?
  if (foundFirst && foundSecond)
  {
    qreal d1 = (r.start()-p1).magnitude();
    qreal d2 = (r.start()-p2).magnitude();
    if (d2 > d1)
    {
      pts[0] = p1;
      pts[1] = p2;
      return;
    }
    else
    {
      pts[1] = p1;
      pts[0] = p2;
      return;
    }
  }

  LineSegment ls3( Point(rect.bottomRight()), Point(rect.topRight()) );
  p = RayLineSegmentIntersect(r, ls3, inf);
  if (inf != NO_INTERSECTION)
  {
    if (!foundFirst)
    {
      foundFirst = true;
      p1 = p;
      info = INTERSECTION;
    }
    else
    {
      foundSecond = true;
      p2 = p;
    }
  }

  // Already found 2 intersections?
  if (foundFirst && foundSecond)
  {
    qreal d1 = (r.start()-p1).magnitude();
    qreal d2 = (r.start()-p2).magnitude();
    if (d2 > d1)
    {
      pts[0] = p1;
      pts[1] = p2;
      return;
    }
    else
    {
      pts[1] = p1;
      pts[0] = p2;
      return;
    }
  }

  LineSegment ls4( Point(rect.bottomRight()), Point(rect.topRight()) );
  p = RayLineSegmentIntersect(r, ls4, inf);
  if (inf != NO_INTERSECTION)
  {
    if (!foundFirst)
    {
      foundFirst = true;
      p1 = p;
      info = INTERSECTION;
    }
    else
    {
      foundSecond = true;
      p2 = p;
    }
  }

  // Found 2 intersections?
  if (foundFirst && foundSecond)
  {
    qreal d1 = (r.start()-p1).magnitude();
    qreal d2 = (r.start()-p2).magnitude();
    if (d2 > d1)
    {
      pts[0] = p1;
      pts[1] = p2;
      return;
    }
    else
    {
      pts[1] = p1;
      pts[0] = p2;
      return;
    }
  }
  else
  {
    pts[0] = p1;
    pts[1] = p2;
  }

}

